home *** CD-ROM | disk | FTP | other *** search
/ Power Programmierung 2 / Power-Programmierung CD 2 (Tewi)(1994).iso / gnu / gnulib / sipp / sphigs / src / src.arj / MAT3MAT.C < prev    next >
Encoding:
C/C++ Source or Header  |  1993-10-12  |  16.1 KB  |  538 lines
  1. /* Copyright 1991, Brown Computer Graphics Group.  All Rights Reserved. */
  2.  
  3. #ifndef lint
  4. static char Version[] = 
  5.    "$Id: MAT3mat.c,v 1.1 1993/01/14 06:01:02 crb Exp $";
  6. #endif
  7.  
  8. /* -------------------------------------------------------------------------
  9.  *
  10.  * Contains functions that operate on matrices, but not on matrix-vector pairs,
  11.  * etc. 
  12.  *
  13.  * ------------------------------------------------------------------------- */
  14.  
  15. #include "mat3defs.h"
  16.  
  17. /* ------------------------ Static Routines  ------------------------------- */
  18.  
  19. /* Determinant of 3x3 matrix with given entries */
  20. #define DET3(A,B,C,D,E,F,G,H,I) \
  21.     ((A*E*I + B*F*G + C*D*H) - (A*F*H + B*D*I + C*E*G))
  22.  
  23. /* -------------------------------------------------------------------------
  24.  * DESCR :    Compute a determinant of a 3 x 3 matrix @mat@, multiplied by
  25.  *         the sign of the permutation (@col1@, @col2@, @col3@). If
  26.  *         (@col1@, @col2@, @col3@ ) = (0, 1, 2), one gets the ordinary
  27.  *         determinant.
  28.  *
  29.  * RETURNS :    The determinant.
  30.  * ------------------------------------------------------------------------- */
  31. static double
  32. MAT3_determinant_lower_3(
  33.    MAT3mat mat,    /* Matrix whose determinant we will compute  */
  34.    int     col1,        /* First entry in permutation */
  35.    int     col2,        /* 2nd entry in permutation */
  36.    int     col3         /* 3rd entry in permutation  */
  37.    )
  38. {
  39.    double det;
  40.  
  41.    det = DET3(mat[1][col1], mat[1][col2], mat[1][col3],
  42.           mat[2][col1], mat[2][col2], mat[2][col3],
  43.           mat[3][col1], mat[3][col2], mat[3][col3]);
  44.  
  45.    return(det);
  46. }
  47.  
  48. /* ------------------------ Private Routines ------------------------------- */
  49.  
  50.  
  51. /* ------------------------ Public Routines  ------------------------------- */
  52. /*LINTLIBRARY*/
  53.  
  54. /* -------------------------------------------------------------------------
  55.  * DESCR   :    Sets @m@ to be the identity matrix.
  56.  *
  57.  * RETURNS :    void
  58.  * ------------------------------------------------------------------------- */
  59. void
  60. MAT3identity(
  61.    MAT3mat m         /* Matrix to be set to identity */
  62.    )
  63. {
  64.     double *mat = (double *)m;
  65.  
  66.     *mat++ = 1.0;
  67.     *mat++ = 0.0;
  68.     *mat++ = 0.0;
  69.     *mat++ = 0.0;
  70.  
  71.     *mat++ = 0.0;
  72.     *mat++ = 1.0;
  73.     *mat++ = 0.0;
  74.     *mat++ = 0.0;
  75.  
  76.     *mat++ = 0.0;
  77.     *mat++ = 0.0;
  78.     *mat++ = 1.0;
  79.     *mat++ = 0.0;
  80.  
  81.     *mat++ = 0.0;
  82.     *mat++ = 0.0;
  83.     *mat++ = 0.0;
  84.     *mat   = 1.0;
  85. }
  86.  
  87. /* -------------------------------------------------------------------------
  88.  * DESCR   :    Sets @m@ to be the zero matrix.
  89.  *
  90.  * RETURNS :    void
  91.  * ------------------------------------------------------------------------- */
  92. void
  93. MAT3zero(
  94.    MAT3mat m         /* Matrix to be set to zero */
  95.    )
  96. {
  97.     double *mat = (double *)m;
  98.  
  99.     *mat++ = 0.0;
  100.     *mat++ = 0.0;
  101.     *mat++ = 0.0;
  102.     *mat++ = 0.0;
  103.  
  104.     *mat++ = 0.0;
  105.     *mat++ = 0.0;
  106.     *mat++ = 0.0;
  107.     *mat++ = 0.0;
  108.  
  109.     *mat++ = 0.0;
  110.     *mat++ = 0.0;
  111.     *mat++ = 0.0;
  112.     *mat++ = 0.0;
  113.  
  114.     *mat++ = 0.0;
  115.     *mat++ = 0.0;
  116.     *mat++ = 0.0;
  117.     *mat   = 0.0;
  118. }
  119.  
  120. /* -------------------------------------------------------------------------
  121.  * DESCR   :    Compute the determinant of @mat@.
  122.  *
  123.  * RETURNS :    The determinant, a double.
  124.  * ------------------------------------------------------------------------- */
  125. double
  126. MAT3determinant(
  127.    MAT3mat mat             /* Matrix whose determinant we compute */
  128.    )
  129. {
  130.    double det;
  131.  
  132.    det = (  mat[0][0] * MAT3_determinant_lower_3(mat, 1, 2, 3)
  133.       - mat[0][1] * MAT3_determinant_lower_3(mat, 0, 2, 3)
  134.       + mat[0][2] * MAT3_determinant_lower_3(mat, 0, 1, 3)
  135.       - mat[0][3] * MAT3_determinant_lower_3(mat, 0, 1, 2));
  136.  
  137.    return(det);
  138. }
  139.  
  140. /* -------------------------------------------------------------------------
  141.  * DESCR   :    Copy the matrix @f@ into the matrix @t@.
  142.  *
  143.  * RETURNS :    void
  144.  * ------------------------------------------------------------------------- */
  145. void
  146. MAT3copy(
  147.    MAT3mat t,    /* Target matrix [output] */
  148.    MAT3mat f    /* Source matrix [input] */
  149. )
  150. {
  151.     double *to = (double *)t;
  152.     double *from = (double *)f;
  153.     
  154.     *to++ = *from++;
  155.     *to++ = *from++;
  156.     *to++ = *from++;
  157.     *to++ = *from++;
  158.  
  159.     *to++ = *from++;
  160.     *to++ = *from++;
  161.     *to++ = *from++;
  162.     *to++ = *from++;
  163.  
  164.     *to++ = *from++;
  165.     *to++ = *from++;
  166.     *to++ = *from++;
  167.     *to++ = *from++;
  168.  
  169.     *to++ = *from++;
  170.     *to++ = *from++;
  171.     *to++ = *from++;
  172.     *to   = *from;
  173. }
  174.  
  175. /* -------------------------------------------------------------------------
  176.  * DESCR   :    Computes the matrix product @mat1@ * @mat2@ and places the
  177.  *        result in @result_mat@. The matrices being multiplied may be the
  178.  *        same as the result, i.e., a call of the form MAT3mult(a,a,b) is
  179.  *        safe.
  180.  *
  181.  * RETURNS :    void
  182.  * ------------------------------------------------------------------------- */
  183. void
  184. MAT3mult(
  185.    MAT3mat      result_mat,    /* The computed product of the next two args  */
  186.    MAT3mat mat1,    /* The first factor of the product */
  187.    MAT3mat mat2     /* The second factor of the product */
  188.    )
  189. {
  190.    int i, j;
  191.    MAT3mat    tmp_mat;
  192.    double    *tmp;
  193.  
  194.    if (((tmp = (double*)result_mat) == (double*)mat1) || 
  195.        (tmp == (double*)mat2))
  196.       tmp = (double*)tmp_mat;
  197.    
  198.    for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
  199.       *(tmp++) = (mat1[i][0] * mat2[0][j] +
  200.           mat1[i][1] * mat2[1][j] +
  201.           mat1[i][2] * mat2[2][j] +
  202.           mat1[i][3] * mat2[3][j]);
  203.  
  204.    if (((double*)result_mat == (double*)mat1) || 
  205.        ((double*)result_mat == (double*)mat2))
  206.       MAT3copy(result_mat, tmp_mat);
  207.  
  208.    return;
  209. }
  210.  
  211. /* -------------------------------------------------------------------------
  212.  * DESCR   :    Compute the transpose of @mat@ and place it in @result_mat@. The
  213.  *        routine may be called with two two arguments equal (i.e.,
  214.  *        MAT3transpose(a, a)) and will still work properly.
  215.  *
  216.  * RETURNS :    void
  217.  * ------------------------------------------------------------------------- */
  218. void
  219. MAT3transpose(
  220.    MAT3mat      result_mat,    /* The computed transpose */
  221.    MAT3mat mat     /* The matrix whose transpose we compute */
  222.    )
  223. {
  224.    int i, j;
  225.    MAT3mat    tmp_mat;
  226.  
  227.    for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) tmp_mat[i][j] = mat[j][i];
  228.  
  229.    MAT3copy(result_mat, tmp_mat);
  230. }
  231.  
  232. /* -------------------------------------------------------------------------
  233.  * DESCR   :    Print the matrix @mat@ on the stream described by the file
  234.  *        point @fp@.
  235.  *
  236.  * RETURNS :    void
  237.  * ------------------------------------------------------------------------- */
  238. void
  239. MAT3print(
  240.    MAT3mat mat,            /* The matrix to be printed */
  241.    FILE    *fp             /* The stream on which to print it */
  242.    )
  243. {
  244.    MAT3print_formatted(mat, fp, CNULL, CNULL, CNULL, CNULL);
  245. }
  246.  
  247. /* -------------------------------------------------------------------------
  248.  * DESCR :    This prints the matrix @mat@ to the file pointer @fp@, using
  249.  *         the format string @format@ to pass to fprintf.  The strings
  250.  *         @head@ and @tail@ are printed at the beginning and end of
  251.  *         each line, and the string @title@ is printed at the top.
  252.  *
  253.  * RETURNS :    void
  254.  * ------------------------------------------------------------------------- */
  255. void
  256. MAT3print_formatted(
  257.    MAT3mat mat,            /* Matrix to be printed out */
  258.    FILE    *fp,            /* File stream to which it is printed */
  259.    char    *title,            /* Title of printout */
  260.    char    *head,            /* Printed before each row of matrix */
  261.    char    *format,        /* Format for entries of matrix */
  262.    char    *tail             /* Printed after each row of matrix */
  263.    )
  264. {
  265.    int i, j;
  266.  
  267.    /* This is to allow this to be called easily from a debugger */
  268.    if (fp == NULL) fp = stderr;
  269.  
  270.    if (title  == NULL)    title  = "MAT3 matrix:\n";
  271.    if (head   == NULL)    head   = "  ";
  272.    if (format == NULL)    format = "%#8.4lf  ";
  273.    if (tail   == NULL)    tail   = "\n";
  274.  
  275.    (void) fprintf(fp, title);
  276.  
  277.    for (i = 0; i < 4; i++) {
  278.       (void) fprintf(fp, head);
  279.       for (j = 0; j < 4; j++) (void) fprintf(fp, format, mat[i][j]);
  280.       (void) fprintf(fp, tail);
  281.    }
  282. }
  283.  
  284. /* -------------------------------------------------------------------------
  285.  * DESCR :    This compares two matrices @m1@ and @m2@ for equality
  286.  *         within @epsilon@.  If all corresponding elements in the
  287.  *         two matrices are equal within @epsilon@, then TRUE is
  288.  *         returned.  Otherwise, FALSE is returned. Epsilon should be
  289.  *         zero or positive.
  290.  *
  291.  * RETURNS :    TRUE if all entries differ pairwise by less than epsilon,
  292.  *         FALSE otherwise.
  293.  * ------------------------------------------------------------------------- */
  294. BOOL
  295. MAT3equal(
  296.    MAT3mat m1,        /* The first matrix to be compared */
  297.    MAT3mat m2,        /* The second matrix to be compared */
  298.    double    epsilon     /* The value within which entries must match */
  299.    )
  300. {
  301.    int i, j;
  302.    double    diff;
  303.  
  304.    /* Special-case for epsilon of 0 */
  305.    if (epsilon == 0.0) {
  306.       for (i = 0; i < 4; i++) for (j = 0; j < 4; j++)
  307.      if (m1[i][j] != m2[i][j]) return(FALSE);
  308.    }
  309.  
  310.    /* Any other epsilon */
  311.    else for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) {
  312.       diff = m1[i][j] - m2[i][j];
  313.       if (! IS_ZERO(diff, epsilon)) return(FALSE);
  314.    }
  315.  
  316.    return TRUE;
  317. }
  318.  
  319. /* -------------------------------------------------------------------------
  320.  * DESCR   :    Compute the trace of @mat@ (i.e., sum of diagonal entries).
  321.  *
  322.  * RETURNS :    The trace of @mat@. 
  323.  * ------------------------------------------------------------------------- */
  324. double
  325. MAT3trace(
  326.    MAT3mat mat        /* Matrix whose trace we are computing */
  327. )
  328. {
  329.    return(mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3]);
  330. }
  331.  
  332. /* -------------------------------------------------------------------------
  333.  * DESCR   :    Raises @mat@ to the power @power@, placing the result in
  334.  *         @result_mat@.
  335.  *
  336.  *
  337.  *
  338.  * RETURNS :    TRUE if the power is non-negative, FALSE otherwise
  339.  *
  340.  * DETAILS :    The computation is performed by starting with the identity
  341.  *         matrix as the result. Then look at the bits of the binary
  342.  *         representation of the power, from left to right. Every time a
  343.  *         1 is encountered, we square the matrix and then multiply the
  344.  *         result by the original matrix. Every time that a 0 is
  345.  *         encountered, just square the matrix.  When done with all the
  346.  *         bits in the power, the matrix will be the correct result
  347.  * ------------------------------------------------------------------------- */
  348. int
  349. MAT3power(
  350.    MAT3mat result_mat,        /* The source matrix raised to the power */
  351.    MAT3mat mat,            /* The source matrix */
  352.    int    power             /* The power to which to raise the source */
  353.    )
  354. {
  355.    int count, rev_power;
  356.    MAT3mat    temp_mat;
  357.  
  358.    if (power < 0) {
  359.       SEVERES("Negative power given to MAT3power:");
  360.       return(FALSE);
  361.    }
  362.    /* Reverse the order of the bits in power to make the looping construct
  363.     * effiecient (and word-size-independent). */
  364.    for (rev_power = count = 0; power > 0; count++, power = power >> 1)
  365.      rev_power = (rev_power << 1) | (power & 1);
  366.    power = rev_power;
  367.  
  368.    MAT3identity(temp_mat);
  369.  
  370.    /* For each bit in power */
  371.    for ( ; count > 0 || rev_power != 0; count--, rev_power = rev_power >> 1) {
  372.  
  373.       /* Square working matrix */
  374.       MAT3mult(temp_mat, temp_mat, temp_mat);
  375.  
  376.       /* If bit is 1, multiply by original matrix */
  377.       if (power & 1) MAT3mult(temp_mat, temp_mat, mat);
  378.    }
  379.  
  380.    MAT3copy(result_mat, temp_mat);
  381.  
  382.    return (TRUE);
  383. }
  384.  
  385.  
  386. /* -------------------------------------------------------------------------
  387.  * DESCR :    Computes the column-reduced form of @mat@ in @result_mat@.
  388.  *         After column reduction, each column's first non-zero entry is
  389.  *         a one.  The leading ones are ordered highest to lowest from
  390.  *         the first column to the last. 
  391.  *
  392.  * RETURNS :    The rank of the matrix, i.e., the number of linearly
  393.  *         independent columns of the original matrix.
  394.  *
  395.  * DETAILS :    Performs the reduction through simple column operations:
  396.  *         multiply a column by a scalar to make its leading value one,
  397.  *         then add a multiple of this column to each of the others to
  398.  *         put zeroes in all other entries of that row. The number
  399.  *         @epsilon@ is used to test near equality with zero.  It should
  400.  *         be non-negative.
  401.  * ------------------------------------------------------------------------- */
  402. int
  403. MAT3column_reduce(
  404.    MAT3mat result_mat,    /* The column-reduced matrix [output] */
  405.    MAT3mat      mat,        /* The original matrix [input] */
  406.    double     epsilon     /* The tolerance for testing against 0 */
  407.    )
  408. {
  409.    int row, col, next_col, r;
  410.    double    t, factor;
  411.  
  412.    MAT3copy(result_mat, mat);
  413.  
  414.    /* next_col is the next column in which a leading one will be placed */
  415.    next_col = 0;
  416.    for (row = 0; row < 4; row++) {
  417.       for (col = next_col; col < 4; col++)
  418.      if (! IS_ZERO(result_mat[row][col], epsilon)) break;
  419.  
  420.       if (col == 4) continue;            /* all 0s, try next row */
  421.       if (col != next_col)            /* swap cols */
  422.      for (r = row; r < 4; r++)
  423.         SWAP(result_mat[r][col], result_mat[r][next_col], t);
  424.  
  425.       /* Make a leading 1 */
  426.       factor = 1.0 / result_mat[row][next_col]; /* make a leading 1 */
  427.       result_mat[row][next_col] = 1.0;
  428.       for (r = row + 1; r < 4; r++) result_mat[r][next_col] *= factor;
  429.  
  430.       for (col = 0; col < 4; col++)        /* zero out cols */
  431.      if (col != next_col) {         /* could skip if already zero */
  432.      factor = -result_mat[row][col];
  433.      result_mat[row][col] = 0.0;
  434.      for (r = row + 1; r < 4; r++)
  435.         result_mat[r][col] += factor * result_mat[r][next_col];
  436.       }
  437.  
  438.       if (++next_col == 4) break;
  439.    }
  440.  
  441.    return(next_col);
  442. }
  443.  
  444.  
  445. /* -------------------------------------------------------------------------
  446.  * DESCR :    This computes the kernel of the transformation represented by
  447.  *         @mat@ from its column-reduced form, and places a basis for
  448.  *         the kernel in the rows of @result_mat@. The number "epsilon"
  449.  *         is used to test near-equality with zero. It should be
  450.  *         non-negative.  If there is no basis (i.e., the matrix is
  451.  *         non-singular), this returns FALSE.
  452.  *
  453.  *
  454.  * RETURNS :    TRUE if the nullity is nonzero, FALSE otherwise. 
  455.  * ------------------------------------------------------------------------- */
  456. int
  457. MAT3kernel_basis(
  458.    MAT3mat result_mat,    /* Stores basis of kernel of mat [output] */
  459.    MAT3mat      mat,        /* Matrix whose kernel we compute [input] */
  460.    double     epsilon     /* Tolerance for comparison with zero [input] */
  461.    )
  462. {
  463.    MAT3mat    reduced;
  464.    int        leading[4],    /* Indices of rows with leading ones    */
  465.         other[4],    /* Indices of other rows        */
  466.         rank,        /* Rank of column-reduced matrix    */
  467.         dim;        /* Dimension of kernel (4 - rank)    */
  468.    int lead_count, other_count, r, c;
  469.  
  470.    /* Column-reduce the matrix */
  471.    rank = MAT3column_reduce(reduced, mat, epsilon);
  472.  
  473.    /* Get dimension of kernel. If zero, there is no basis */
  474.    if ((dim = 4 - rank) == 0) return(FALSE);
  475.  
  476.    /* If matrix transforms all points to origin, return identity */
  477.    if (rank == 0) MAT3identity(result_mat);
  478.  
  479.    else {
  480.       /* Find rows with leading ones, and other rows */
  481.       lead_count = other_count = 0;
  482.       for (r = 0; r < 4; r++) {
  483.      if (reduced[r][lead_count] == 1.0) leading[lead_count++] = r;
  484.      else                    other[other_count++]  = r;
  485.       }
  486.  
  487.       /* Put identity into rows with no leading ones */
  488.       for (r = 0; r < dim; r++) for (c = 0; c < other_count; c++)
  489.      result_mat[r][other[c]] = (r == c ? -1.0 : 0.0);
  490.  
  491.       /* Copy other values */
  492.       for (r = 0; r < dim; r++) for (c = 0; c < rank; c++)
  493.      result_mat[r][leading[c]] = reduced[other[r]][c];
  494.    }
  495.    return(TRUE);
  496. }
  497.  
  498. /* -------------------------------------------------------------------------
  499.  * DESCR   :    Multiply matrix @m@ by the real number @scalar@ and put the
  500.  *         result in @result_mat@.  
  501.  *
  502.  * RETURNS :    void
  503.  * ------------------------------------------------------------------------- */
  504. void
  505. MAT3scalar_mult(
  506.    MAT3mat result_mat,    /* The result of scaling the matrix [output] */
  507.    MAT3mat          m,        /* Matrix to be scaled [input] */
  508.    double           scalar     /* Scaling factor [input] */
  509.    )
  510. {
  511.    int i,j;
  512.  
  513.    for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) {
  514.       result_mat[i][j] = scalar * m[i][j];
  515.    }
  516. }
  517.  
  518. /* -------------------------------------------------------------------------
  519.  * DESCR   :    Compute the sum of matrices @A@ and @B@ and place the result
  520.  *         in @result_mat@, which may be one of @A@ and @B@ without any
  521.  *         fear of errors.
  522.  *
  523.  * RETURNS :    void
  524.  * ------------------------------------------------------------------------- */
  525. void
  526. MAT3mat_add(
  527.    MAT3mat result_mat,    /* The sum of the matrices [output] */
  528.    MAT3mat A,            /* The first summand [input] */
  529.    MAT3mat B             /* The second summand [input] */
  530.    )
  531. {
  532.    int i,j;
  533.  
  534.    for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) {
  535.       result_mat[i][j] = A[i][j] + B[i][j];
  536.    }
  537. }
  538.